#include "fedtime.hh"
#include "HwFederateAmbassador.hh"
#include <iostream>
#include "Utils.h"

using namespace std;

extern RTI::InteractionClassHandle hSumoClass;
extern RTI::ParameterHandle hSumoId;
extern RTI::ParameterHandle hSumoSpeed;
extern RTI::ParameterHandle hSumoAcceleration;
extern RTI::ParameterHandle hSumoX;
extern RTI::ParameterHandle hSumoY;

extern RTI::InteractionClassHandle hSyncClass;
extern RTI::ParameterHandle hSync;
extern RTI::ParameterHandle hNsOnline;

extern RTIfedTime g_currentTime;
extern bool g_bConstrained;
extern bool g_bRegulation;
extern bool g_granted;

extern bool socketOpen;

HwFederateAmbassador::HwFederateAmbassador() {
    ////----初始化 sendCarParams vector
    sendCarParams.clear();
    sendCarParams.push_back("sumoId");
    sendCarParams.push_back("sumoSpeed");
    sendCarParams.push_back("sumoAcc");
    sendCarParams.push_back("sumoX");
    sendCarParams.push_back("sumoY");

    sendSyncInfoParams.clear();
    sendSyncInfoParams.push_back("sumoTime");
    sendSyncInfoParams.push_back("infoNum");
    sendSyncInfoParams.push_back("nsOnline");

    if (!socketOpen) {
        return;
    }

    ////----创建和 NS3 的连接----
    LOG(INFO) << "NS3 RTI 成员已上线";
    // 1.创建通信的套接字
    listener = socket(AF_INET, SOCK_STREAM, 0);
    if (listener == -1) {
        LOG(ERROR) << "连接NS3失败，检查NS3是否启动";
        exit(201);
    }

    // 连接服务器本地的IP port
    addr.sin_family = AF_INET;
    inet_pton(AF_INET, "127.0.0.1", &addr.sin_addr.s_addr);
    addr.sin_port = htons(port);
    int ret = connect(listener, (struct sockaddr *) &addr, sizeof(addr));
    if (ret == -1) {
        LOG(ERROR) << "连接NS3失败，检查NS3是否启动";
        exit(202);
    } else {
        LOG(INFO) << "连接NS3成功" << endl;
    }

    // ////////////////////////////////////
    // // 创建tcp原始套接字listener,注意这里我们将使用套接字的默认行为阻塞模式：即一直阻塞到请求动作完成为止
    // // 例如至少接收到一条来自客户端的消息，recv函数才会返回，当然，与此同时具有被阻塞函数的进程将会被
    // // 操作系统挂起（另一种非阻塞模式这里不使用）TCP/IP socket编程P100
    // listener = socket(AF_INET, SOCK_STREAM, 0);

    // /*
    //  * 当返回值小于0，输出错误消息socket，退出函数告诉系统程序已完成，导致程序终止进程。
    //  * 状态参数(status=1)是程序的退出状态，它成为进程终止状态的一部分。 该功能不返回。
    //  */
    // if (listener < 0)
    // {
    //     LOG(ERROR) << "连接 NS3 失败";
    //     exit(101);
    // }

    // // 确定tcpip地址族
    // addr.sin_family = AF_INET;

    // // 该函数将整型从主机字节顺序转换为网络字节顺序（网络的端口号要转变为实际网络可识别的顺序）
    // // 以此来定义端口号 9999
    // addr.sin_port = htons(port);

    // // 该函数将uint32_t整数主机从主机字节顺序转换为网络字节顺序。这用于IPv4 Internet地址定义。
    // addr.sin_addr.s_addr = INADDR_ANY;

    // /*
    //  * bind函数为套接字分配一个本地地址。 addr和length参数指定地址; 地址的详细格式取决于命名空间。
    //  * 地址的第一部分始终是指定名称空间的格式指示符，并指出该地址采用该名称空间的格式。
    //  * 返回值在成功时为0，在失败时为-1。
    //  */
    // if (bind(listener, (struct sockaddr *)&addr, sizeof(addr)) < 0)
    // {
    //     LOG(ERROR) << "连接 NS3 失败";// 返回错误信息bind，表示bind失败
    //     exit(102);
    // }

    // /*
    //  * listen函数使套接字能够接受连接，从而使其成为服务器套接字。参数n为未决连接指定队列的长度。
    //  * 当队列填满时，尝试连接的新客户端将以ECONNREFUSED失败，直到服务器调用接受函数接受来自队列的连接。
    //  * listen函数在成功时返回0，在失败时返回-1。
    //  */
    // // 原始套接字开始侦听
    // LOG(INFO) << "等待 NS3 上线连接...";
    // if (listen(listener, 128) < 0)
    // {
    //     LOG(ERROR) << "连接 NS3 失败";
    //     exit(103);
    // }

    // struct sockaddr_in caddr;
    // socklen_t addrlen = sizeof(caddr);
    // accepter = accept(listener, (struct sockaddr *)&caddr, &addrlen);
    // if (accepter < 0)
    // {
    //     LOG(ERROR) << "连接 NS3 失败";
    //     exit(104);
    // }
    // else
    // {
    //     LOG(INFO) << "连接 NS3 成功";
    //     char ip[32];
    //     printf("NS3 RTI 成员的IP：%s，端口：%d\n",
    //            inet_ntop(AF_INET, &caddr.sin_addr.s_addr, ip, sizeof(ip)),
    //            ntohs(caddr.sin_port));
    // }
}

HwFederateAmbassador::~HwFederateAmbassador() throw(RTI::FederateInternalError) {
}

////////////////////////////////////
// Federation Management Services //
////////////////////////////////////

void HwFederateAmbassador::synchronizationPointRegistrationSucceeded(
        const char *label) // supplied C4)
throw(
RTI::FederateInternalError) {
}

void HwFederateAmbassador::synchronizationPointRegistrationFailed(
        const char *label) // supplied C4)
throw(
RTI::FederateInternalError) {
}

void HwFederateAmbassador::announceSynchronizationPoint(
        const char *label, // supplied C4
        const char *tag)   // supplied C4
throw(
RTI::FederateInternalError) {
}

void HwFederateAmbassador::federationSynchronized(
        const char *label) // supplied C4)
throw(
RTI::FederateInternalError) {
}

void HwFederateAmbassador::initiateFederateSave(
        const char *label) // supplied C4
throw(
RTI::UnableToPerformSave,
RTI::FederateInternalError) {
}

void HwFederateAmbassador::federationSaved() throw(
RTI::FederateInternalError) {
}

void HwFederateAmbassador::federationNotSaved() throw(
RTI::FederateInternalError) {
}

void HwFederateAmbassador::requestFederationRestoreSucceeded(
        const char *label) // supplied C4
throw(
RTI::FederateInternalError) {
}

void HwFederateAmbassador::requestFederationRestoreFailed(
        const char *label) // supplied C4
throw(
RTI::FederateInternalError) {
}

void HwFederateAmbassador::requestFederationRestoreFailed(
        const char *label,
        const char *reason) // supplied C4
throw(
RTI::FederateInternalError) {
}

void HwFederateAmbassador::federationRestoreBegun() throw(
RTI::FederateInternalError) {
}

void HwFederateAmbassador::initiateFederateRestore(
        const char *label,          // supplied C4
        RTI::FederateHandle handle) // supplied C1
throw(
RTI::SpecifiedSaveLabelDoesNotExist,
RTI::CouldNotRestore,
RTI::FederateInternalError) {
}

void HwFederateAmbassador::federationRestored() throw(
RTI::FederateInternalError) {
}

void HwFederateAmbassador::federationNotRestored() throw(
RTI::FederateInternalError) {
}

/////////////////////////////////////
// Declaration Management Services //
/////////////////////////////////////

void HwFederateAmbassador::startRegistrationForObjectClass(
        RTI::ObjectClassHandle theClass) // supplied C1
throw(
RTI::ObjectClassNotPublished,
RTI::FederateInternalError) {
}

void HwFederateAmbassador::stopRegistrationForObjectClass(
        RTI::ObjectClassHandle theClass) // supplied C1
throw(
RTI::ObjectClassNotPublished,
RTI::FederateInternalError) {
}

void HwFederateAmbassador::turnInteractionsOn(
        RTI::InteractionClassHandle theHandle) // supplied C1
throw(
RTI::InteractionClassNotPublished,
RTI::FederateInternalError) {
}

void HwFederateAmbassador::turnInteractionsOff(
        RTI::InteractionClassHandle theHandle) // supplied C1
throw(
RTI::InteractionClassNotPublished,
RTI::FederateInternalError) {
}

////////////////////////////////
// Object Management Services //
////////////////////////////////

void HwFederateAmbassador::discoverObjectInstance(
        RTI::ObjectHandle theObject,           // supplied C1
        RTI::ObjectClassHandle theObjectClass, // supplied C1
        const char *theObjectName)             // supplied C4
throw(
RTI::CouldNotDiscover,
RTI::ObjectClassNotKnown,
RTI::FederateInternalError) {
}

void HwFederateAmbassador::reflectAttributeValues(
        RTI::ObjectHandle theObject,                           // supplied C1
        const RTI::AttributeHandleValuePairSet &theAttributes, // supplied C4
        const RTI::FedTime &theTime,                           // supplied C1
        const char *theTag,                                    // supplied C4
        RTI::EventRetractionHandle theHandle)                  // supplied C1
throw(
RTI::ObjectNotKnown,
RTI::AttributeNotKnown,
RTI::FederateOwnsAttributes,
RTI::InvalidFederationTime,
RTI::FederateInternalError) {
}

void HwFederateAmbassador::reflectAttributeValues(
        RTI::ObjectHandle theObject,                           // supplied C1
        const RTI::AttributeHandleValuePairSet &theAttributes, // supplied C4
        const char *theTag)                                    // supplied C4
throw(
RTI::ObjectNotKnown,
RTI::AttributeNotKnown,
RTI::FederateOwnsAttributes,
RTI::FederateInternalError) {
}

void HwFederateAmbassador::receiveInteraction(
        RTI::InteractionClassHandle theInteraction,            // supplied C1
        const RTI::ParameterHandleValuePairSet &theParameters, // supplied C4
        const RTI::FedTime &theTime,                           // supplied C4
        const char *theTag,                                    // supplied C4
        RTI::EventRetractionHandle theHandle)                  // supplied C1
throw(
RTI::InteractionClassNotKnown,
RTI::InteractionParameterNotKnown,
RTI::InvalidFederationTime,
RTI::FederateInternalError) {
    // call the next service.
    this->receiveInteraction(theInteraction, theParameters, theTag);
}

void HwFederateAmbassador::receiveInteraction(
        RTI::InteractionClassHandle theInteraction,            // supplied C1
        const RTI::ParameterHandleValuePairSet &theParameters, // supplied C4
        const char *theTag)                                    // supplied C4
throw(
RTI::InteractionClassNotKnown,
RTI::InteractionParameterNotKnown,
RTI::FederateInternalError) {

    RTI::ParameterHandle paraHandle;
    RTI::ULong valueLength = 0;
    string value;
    string sendMsg;
    nlohmann::ordered_json jMsg;

    if (strcmp(theTag, "info_head") == 0) {
        sentCount = 0;
        string vehNum;
        string sumoTime;

        vehNum.resize(theParameters.getValueLength(0));
        theParameters.getValue(0, (char *) vehNum.c_str(), valueLength);
        needSendCount = atoi(vehNum.c_str());

        sumoTime.resize(theParameters.getValueLength(0));
        theParameters.getValue(0, (char *) sumoTime.c_str(), valueLength);

        LOG(DEBUG) << "接收到 t_InfoNum: " << needSendCount << ", sumoTime:" << sumoTime;

        jMsg["sumoTime"] = stof(sumoTime);
        jMsg["vehNum"] = vehNum;
        cout << jMsg.dump() << endl;

        // 发送数据
        if (!socketOpen) {
            return;
        }
        sendMsg = jMsg.dump();
        char *buff = (char *) sendMsg.c_str();
        this_thread::sleep_for(chrono::milliseconds(25));
        if (send(listener, buff, strlen(buff) + 1, 0) <= 0) {
            exit(999);
        }

        std::memset(buff, 0, sizeof(buff));
    } else if (strcmp(theTag, "sumo_car") == 0) {
//        cout << "size: " << theParameters.size();
//        for (int i = 0; i < theParameters.size(); i++) {
//            // 更改字符串的长度为参数值的长度
//            value.resize(theParameters.getValueLength(i));
//            theParameters.getValue(i, (char *) value.c_str(), valueLength);
//            cout << value << ", ";
//        }
//        cout << endl;

        jMsg["id"] = Utils::Transform::rtiValueToString(theParameters, 0, &value, valueLength);
        jMsg["speed"] = Utils::Transform::rtiValueToString(theParameters, 4, &value, valueLength);
        jMsg["x"] = Utils::Transform::rtiValueToString(theParameters, 5, &value, valueLength);
        jMsg["y"] = Utils::Transform::rtiValueToString(theParameters, 6, &value, valueLength);
        jMsg["z"] = Utils::Transform::rtiValueToString(theParameters, 7, &value, valueLength);

        sentCount++;
        needSendCount--;
        allCarDataArray.push_back(jMsg);
        if (needSendCount == 0) {
            string t_s = allCarDataArray.dump();
            cout << t_s << endl;
            if (!socketOpen) {
                return;
            }

            char *buff = (char *) t_s.c_str();
            this_thread::sleep_for(chrono::milliseconds(20));
            if (send(listener, buff, strlen(buff) + 1, 0) <= 0) {
                exit(999);
            }

            std::memset(buff, 0, sizeof(buff));
            allCarDataArray.clear();
            // this_thread::sleep_for(chrono::milliseconds(10));
        }
    }
    // 毫秒级
    // this_thread::sleep_for(chrono::milliseconds(5));
    // 微妙级
    // this_thread::sleep_for(chrono::microseconds(10));
    // LOG(DEBUG) << "等待...";
}

void HwFederateAmbassador::removeObjectInstance(
        RTI::ObjectHandle theObject,          // supplied C1
        const RTI::FedTime &theTime,          // supplied C4
        const char *theTag,                   // supplied C4
        RTI::EventRetractionHandle theHandle) // supplied C1
throw(
RTI::ObjectNotKnown,
RTI::InvalidFederationTime,
RTI::FederateInternalError) {
}

void HwFederateAmbassador::removeObjectInstance(
        RTI::ObjectHandle theObject, // supplied C1
        const char *theTag)          // supplied C4
throw(
RTI::ObjectNotKnown,
RTI::FederateInternalError) {
}

void HwFederateAmbassador::attributesInScope(
        RTI::ObjectHandle theObject,                  // supplied C1
        const RTI::AttributeHandleSet &theAttributes) // supplied C4
throw(
RTI::ObjectNotKnown,
RTI::AttributeNotKnown,
RTI::FederateInternalError) {
}

void HwFederateAmbassador::attributesOutOfScope(
        RTI::ObjectHandle theObject,                  // supplied C1
        const RTI::AttributeHandleSet &theAttributes) // supplied C4
throw(
RTI::ObjectNotKnown,
RTI::AttributeNotKnown,
RTI::FederateInternalError) {
}

void HwFederateAmbassador::provideAttributeValueUpdate(
        RTI::ObjectHandle theObject,                  // supplied C1
        const RTI::AttributeHandleSet &theAttributes) // supplied C4
throw(
RTI::ObjectNotKnown,
RTI::AttributeNotKnown,
RTI::AttributeNotOwned,
RTI::FederateInternalError) {
}

void HwFederateAmbassador::turnUpdatesOnForObjectInstance(
        RTI::ObjectHandle theObject,                  // supplied C1
        const RTI::AttributeHandleSet &theAttributes) // supplied C4
throw(
RTI::ObjectNotKnown,
RTI::AttributeNotOwned,
RTI::FederateInternalError) {
}

void HwFederateAmbassador::turnUpdatesOffForObjectInstance(
        RTI::ObjectHandle theObject,                  // supplied C1
        const RTI::AttributeHandleSet &theAttributes) // supplied C4
throw(
RTI::ObjectNotKnown,
RTI::AttributeNotOwned,
RTI::FederateInternalError) {
}

///////////////////////////////////
// Ownership Management Services //
///////////////////////////////////

void HwFederateAmbassador::requestAttributeOwnershipAssumption(
        RTI::ObjectHandle theObject,                      // supplied C1
        const RTI::AttributeHandleSet &offeredAttributes, // supplied C4
        const char *theTag)                               // supplied C4
throw(
RTI::ObjectNotKnown,
RTI::AttributeNotKnown,
RTI::AttributeAlreadyOwned,
RTI::AttributeNotPublished,
RTI::FederateInternalError) {
}

void HwFederateAmbassador::attributeOwnershipDivestitureNotification(
        RTI::ObjectHandle theObject,                       // supplied C1
        const RTI::AttributeHandleSet &releasedAttributes) // supplied C4
throw(
RTI::ObjectNotKnown,
RTI::AttributeNotKnown,
RTI::AttributeNotOwned,
RTI::AttributeDivestitureWasNotRequested,
RTI::FederateInternalError) {
}

void HwFederateAmbassador::attributeOwnershipAcquisitionNotification(
        RTI::ObjectHandle theObject,                      // supplied C1
        const RTI::AttributeHandleSet &securedAttributes) // supplied C4
throw(
RTI::ObjectNotKnown,
RTI::AttributeNotKnown,
RTI::AttributeAcquisitionWasNotRequested,
RTI::AttributeAlreadyOwned,
RTI::AttributeNotPublished,
RTI::FederateInternalError) {
}

void HwFederateAmbassador::attributeOwnershipUnavailable(
        RTI::ObjectHandle theObject,                  // supplied C1
        const RTI::AttributeHandleSet &theAttributes) // supplied C4
throw(
RTI::ObjectNotKnown,
RTI::AttributeNotKnown,
RTI::AttributeAlreadyOwned,
RTI::AttributeAcquisitionWasNotRequested,
RTI::FederateInternalError) {
}

void HwFederateAmbassador::requestAttributeOwnershipRelease(
        RTI::ObjectHandle theObject,                        // supplied C1
        const RTI::AttributeHandleSet &candidateAttributes, // supplied C4
        const char *theTag)                                 // supplied C4
throw(
RTI::ObjectNotKnown,
RTI::AttributeNotKnown,
RTI::AttributeNotOwned,
RTI::FederateInternalError) {
}

void HwFederateAmbassador::confirmAttributeOwnershipAcquisitionCancellation(
        RTI::ObjectHandle theObject,                  // supplied C1
        const RTI::AttributeHandleSet &theAttributes) // supplied C4
throw(
RTI::ObjectNotKnown,
RTI::AttributeNotKnown,
RTI::AttributeAlreadyOwned,
RTI::AttributeAcquisitionWasNotCanceled,
RTI::FederateInternalError) {
}

void HwFederateAmbassador::informAttributeOwnership(
        RTI::ObjectHandle theObject,       // supplied C1
        RTI::AttributeHandle theAttribute, // supplied C1
        RTI::FederateHandle theOwner)      // supplied C1
throw(
RTI::ObjectNotKnown,
RTI::AttributeNotKnown,
RTI::FederateInternalError) {
}

void HwFederateAmbassador::attributeIsNotOwned(
        RTI::ObjectHandle theObject,       // supplied C1
        RTI::AttributeHandle theAttribute) // supplied C1
throw(
RTI::ObjectNotKnown,
RTI::AttributeNotKnown,
RTI::FederateInternalError) {
}

void HwFederateAmbassador::attributeOwnedByRTI(
        RTI::ObjectHandle theObject,       // supplied C1
        RTI::AttributeHandle theAttribute) // supplied C1
throw(
RTI::ObjectNotKnown,
RTI::AttributeNotKnown,
RTI::FederateInternalError) {
}

//////////////////////////////
// Time Management Services //
//////////////////////////////

// 启用时间限制。RTI同意将仿真成员设置为时间管理受限的成员
void HwFederateAmbassador::timeConstrainedEnabled(
        const RTI::FedTime &theFederateTime) // supplied C4
throw(
RTI::InvalidFederationTime,
RTI::EnableTimeConstrainedWasNotPending,
RTI::FederateInternalError) {
    g_currentTime = theFederateTime;
    g_bConstrained = true;
    // LOG(INFO) << "RTI同意将仿真成员设置为时间管理受限的成员";
    // LOG(INFO) << "timeConstrainedEnabled: " << theFederateTime << endl;
}

// 启用时间调节。RTI同意将仿真成员设置为时间管控成员
void HwFederateAmbassador::timeRegulationEnabled(
        const RTI::FedTime &theFederateTime) // supplied C4
throw(
RTI::InvalidFederationTime,
RTI::EnableTimeRegulationWasNotPending,
RTI::FederateInternalError) {
    g_currentTime = theFederateTime;
    g_bRegulation = true;
    LOG(INFO) << "RTI同意将仿真成员设置为时间管控成员";
    LOG(INFO) << "timeRegulationEnabled: " << theFederateTime;
}

void HwFederateAmbassador::timeAdvanceGrant(
        const RTI::FedTime &theTime) // supplied C4
throw(
RTI::InvalidFederationTime,
RTI::TimeAdvanceWasNotInProgress,
RTI::FederateInternalError) {
    int adFlag = 0;
    RTIfedTime tmpTime = theTime;
    g_currentTime = theTime;
    g_granted = true;
    LOG(TRACE) << "RTI同意仿真成员推进到: " << theTime;
}

void HwFederateAmbassador::requestRetraction(
        RTI::EventRetractionHandle theHandle) // supplied C1
throw(
RTI::EventNotKnown,
RTI::FederateInternalError) {
}
